#ifndef __COSTMAP_2D_H__
#define __COSTMAP_2D_H__

#include "common/costmap_base.h"
#include "layers/static_layer.h"
#include "layers/sensors_layer.h"
#include "layers/inflation_layer.h"


namespace costmap_2d{

class Costmap2D{
    public: 
        Costmap2D(string name, bool is_local, tf2_ros::Buffer* tf_buffer);
        ~Costmap2D();

        // 参数初始化
        void GlobalParamsInit();
        void LocalParamsInit();

        // 设置地图数据
        void setMapData(nav_msgs::OccupancyGrid::Ptr map_ptr);

        // 设置点云数据
        void setLidar2DScanData(sensor_msgs::LaserScan::Ptr sensor_ptr);
        void setPointCloud2Data(sensor_msgs::PointCloud2::Ptr sensor_ptr);

        // 获取代价地图
        boost::shared_ptr<Costmap> getCostMap();
        // 获取栅格地图
        void getOccupancyGrid(nav_msgs::OccupancyGrid& map_msg);

        // 更新代价地图
        void updateCostmap();
        // 清除代价地图
        void clearCostmap();

    private:
        string name_;
        bool isLocal_;
        tf2_ros::Buffer* const tfBuffer_;
        boost::shared_ptr<Costmap> originCostmapPtr_;       //全局静态地图
        boost::shared_ptr<Costmap> realtimeCostmapPtr_;     //实时代价地图
        boost::shared_ptr<StaticLayer> staticLayerPtr_;
        boost::shared_ptr<SensorLayer> sensorLayerPtr_;
        boost::shared_ptr<InflationLayer> inflationLayerPtr_;
        
};

}; //costmap_2d
#endif